import lejos.nxt.*;
import lejos.robotics.navigation.*;

public class master {
	private float Distance;
	TachoPilot p;
	int angle = 45;
	wallfollow WallFollow;
	muurvolger MuurVolger;
	static master m = new master();
	Thread t;
	
	public static void main(String[] args)
	{
		// Nieuwe muurvolger starten.
		m.go();
	}

	public master(){
		p = new TachoPilot(2.184f, 4.233f, Motor.A, Motor.C,true);
		t = new Thread(new SonicThread());
		p.setMoveSpeed(12f);
	}
	private void go(){
		WallFollow = new wallfollow(p,this);
		MuurVolger = new muurvolger(p,this);
		t.start();
		try {
			Thread.sleep(1000);
		} catch (InterruptedException e1) {
		}
//		changeWallSide(false);
		while(true){
			System.out.println("Straight");
			try{
				Thread.sleep(1000);
			}catch(InterruptedException e){
			}
			MuurVolger.pause();
			System.out.println("Straight");
			WallFollow.restart();
			System.out.println("Wall");
			try{
				Thread.sleep(1000);
			}catch(InterruptedException e){
			}
			WallFollow.pause();
			MuurVolger.restart();
			System.out.println("Wall");
		}
	}

	private void changeWallSide(boolean left){
		WallFollow.pause();
		WallFollow.setSide(false);
		Motor.B.rotate(WallFollow.getSide()*angle);
		try{
			Thread.sleep(1000);
		}catch(InterruptedException e){
		}
		WallFollow.restart();
	}
	
	class SonicThread implements Runnable {
		public void run(){
			final UltrasonicSensor sonic = new UltrasonicSensor(SensorPort.S2);
			float[] distances;
			distances = new float[10];
			while(true){
				for(int i = 9; i<0;i--){
					distances[i]=distances[i-1];
				}

				distances[0] = sonic.getRange(); 

				try{
					Thread.sleep(2);
				}catch(InterruptedException e){
				}

				// Van de bekomen 10 meetwaarden wordt dan de kleinste berekend,
				// gezien die relatief gezien het meest nauwkeurig zal zijn.
				float smallest = distances[0];
				for(int i = 1; i<distances.length;i++){
					if(distances[i]<=smallest && distances[i] > 5)
						smallest = distances[i];
				}

				Distance = smallest;
			}
		}
	}
	
	public float getDistance(){
		return this.Distance;
	}
}
